#include <cv.h>
#include <highgui.h>

#include "faceDetector.h"

using namespace cv;

//TODO:
//CONSIDER
//Morphological ops							impl, untest
//Backsub
//TorsoROI									done, tested somewhat
//Shrunken faceROI?							done, tested somewhat, works well
		//face region tends to be too wide
//Patchbased
//thresholding histos
//Mean-shift seg to update histo
//Edge preserving smoothing
//Adaptive thresholding

//meanshift + basic init guess				done, tested somewhat, has problems
//meanshift + init guess

//camshift, init guess : almost all frame	done, tested somewhat, end frame a bit large?

//FIX: Deallocation! Check.

Point initialLocationGuess( const Mat& probImage );

int main( int argc, char** argv )
{
    Mat src;
    if( argc != 2 || !(src=imread(argv[1], 1)).data )
        return -1;
	
    Mat hsvFullSize;
    cvtColor(src, hsvFullSize, CV_BGR2HSV);
	
	FaceDetector* fd = new FaceDetector(1.0);
	
	vector<Rect>* faces = fd->detectFace(src);
	
	if (!faces->empty()) {
		
		Rect faceRect = faces->front();
		
		Rect narrowFaceRect = Rect( faceRect.x+faceRect.width/6, faceRect.y, 2*faceRect.width/3, faceRect.height);
		
		//Draw full face rect
		rectangle( src, Point( faceRect.x, faceRect.y ), //WAS bug : cvRectangle instead
				  Point( faceRect.x+faceRect.width, faceRect.y + faceRect.height ),
				  Scalar::all(0),
				  2);		
		//Draw narrow face rect
		rectangle( src, Point( narrowFaceRect.x, narrowFaceRect.y ), //WAS bug : cvRectangle instead
				  Point( narrowFaceRect.x+narrowFaceRect.width, narrowFaceRect.y + narrowFaceRect.height ),
				  Scalar::all(100),
				  2);
	
		Mat hsvFace( hsvFullSize, narrowFaceRect );
		
		// let's quantize the hue to 30 levels
		// and the saturation to 32 levels
		int hbins = 30, sbins = 32;
		int histSize[] = {hbins, sbins};
		// hue varies from 0 to 179, see cvtColor
		float hranges[] = { 0, 180 };
		// saturation varies from 0 (black-gray-white) to
		// 255 (pure spectrum color)
		float sranges[] = { 0, 256 };
		const float* ranges[] = { hranges, sranges };
		MatND hist;
		// we compute the histogram from the 0-th and 1-st channels
		int channels[] = {0, 1};
		//int *channels2 = &channels[0];
		
		calcHist( &hsvFace, 1, channels, Mat(), // do not use mask
				 hist, 2, histSize, ranges,
				 true, // the histogram is uniform
				 false );
		double maxVal = 0;
		minMaxLoc(hist, 0, &maxVal, 0, 0);
		
		int scale = 10;
		Mat histImg = Mat::zeros(sbins*scale, hbins*10, CV_8UC3);
		
		for( int h = 0; h < hbins; h++ )
			for( int s = 0; s < sbins; s++ )
			{
				float binVal = hist.at<float>(h, s);
				int intensity = cvRound(binVal*255/maxVal); //WAS bug : maxValue instead of maxVal
				rectangle( histImg, Point(h*scale, s*scale), //WAS bug : cvRectangle instead
							Point( (h+1)*scale - 1, (s+1)*scale - 1),
							Scalar::all(intensity),
							CV_FILLED );
			}
		
		//Back proj
		
		//Torso start set to 1 face width to the left of face start,
		//or if that's outside the frame, just 0.
		//NOTE: Not using narrowFaceRect sizes!
		//TODO: Determine good torso size
		int torsoApproxStartX = ( faceRect.x > faceRect.width ) ? faceRect.x - faceRect.width : 0;
		
		
		//Torso width set to 3 face width, or maximum possible,
		//if that is too much to fit inside frame.
		int torsoApproxWidth = ( src.cols > 3*faceRect.width ) ? 3*faceRect.width : src.cols - torsoApproxStartX;
		
		//FIX : throw exception if face is too near bottom?
		int neckTop = (src.rows > faceRect.y + 3*faceRect.width/2) ? faceRect.y + 3*faceRect.width/2 : faceRect.y;
		
		Rect torsoApproxRect( torsoApproxStartX, neckTop,
							torsoApproxWidth, src.rows - neckTop);
		
		
		rectangle( src, Point( torsoApproxStartX, neckTop ), //WAS bug : cvRectangle instead
				  Point( torsoApproxStartX + torsoApproxWidth, src.rows ),
				  Scalar::all(0),
				  2);
		
		Mat hsvTorso( hsvFullSize, torsoApproxRect);
		
		Mat backProj = Mat::zeros(hsvTorso.cols, hsvTorso.rows, CV_8UC1);
		
		std::cout << "to here 2" << std::endl;
		
		calcBackProject( &hsvTorso, 1, channels, hist, backProj, ranges, 1.0, true);
		
		namedWindow( "BackProj", 1 );
		imshow( "BackProj", backProj );
		
		//waitKey();

		//FIX : check in-place support
		morphologyEx( backProj, backProj, MORPH_OPEN, Mat(), Point(-1,-1), 1);
		//OPEN seems good
		
		//How many iter?
		
		//7 a bit many?
		
		//3 too many?
		
		//1 appears best,
		//at least on handL_back_dave.jpg,
		//while still removing most noise on handL_wbg6_fSTD_long.jpg
		
		//TODO : It appears to be worthwhile, but do test.
		

		
		
		//Finding the hand location!
		
		//NOTE, empirically, it seems a good bounding box for the hand is the full size face rect
		//BUT we need something much bigger?! We don't know where it starts off
		//OR : we could compute the starting location as a weighted average
		//OR : we could just use this starting location, if it's good. Probably not...
		//FIX : find good starting point!
		//FIX : ensure edge stability!!!
		//FIX : should use meanshift, camshift or just starting point?
		
		//FIX : bad sizes!!!
//		int handWidth = ( backProj.cols < faceRect.width ) ? backProj.cols : faceRect.width;
//		
//		int handHeight = ( backProj.rows < faceRect.height ) ? backProj.rows : faceRect.height;
//		
//		
//		Rect handRect = Rect( backProj.cols/2 - handWidth/2 , backProj.rows/2 - handHeight/2, handWidth , handHeight );
		
//		const int initialHandRectBorder = 0;
//		
//		
//		
//		Rect handRect = Rect( initialHandRectBorder, initialHandRectBorder,
//							 torsoApproxRect.width - initialHandRectBorder, torsoApproxRect.height - initialHandRectBorder );
		
		//FIX : find appropriate numbers!
		
		//CamShift( backProj, handRect, TermCriteria( TermCriteria::MAX_ITER + TermCriteria::EPS , 60, 0.001) );
		
		Point handCenterGuess = initialLocationGuess( backProj );
		
		Rect handRect = Rect( handCenterGuess.x - faceRect.width/2 , handCenterGuess.y - faceRect.height/2 ,
							 faceRect.width , faceRect.height );
		
		//Draw grey first-guess triangle
		rectangle( backProj, Point( handRect.x, handRect.y ), //WAS bug : cvRectangle instead
				  Point( handRect.x + handRect.width, handRect.y+handRect.height),
				  Scalar::all(100),
				  2);
		
		meanShift( backProj, handRect, TermCriteria( TermCriteria::MAX_ITER + TermCriteria::EPS , 60, 0.001) );
			//moves handRect to local max
		
		//TODO : TEST
		
		if (handRect.x < 0) {
			handRect.x = 0;
		} else if ( backProj.cols < handRect.x + handRect.width ) {
			handRect.x = backProj.cols - handRect.width;
		}
		
		if (handRect.y < 0) {
			handRect.y = 0;
		} else if ( backProj.rows < handRect.y + handRect.height ) {
			handRect.y = backProj.rows - handRect.height;
		}
		
		rectangle( backProj, Point( handRect.x, handRect.y ), //WAS bug : cvRectangle instead
				  Point( handRect.x + handRect.width, handRect.y+handRect.height),
				  Scalar::all(250),
				  2);
		
		//TODO : reject if not hand shaped !
		//TODO : more advanced rejection techniques?
		
		namedWindow( "Source", 1 );
		imshow( "Source", src );
		
		namedWindow( "H-S Histogram", 1 );
		imshow( "H-S Histogram", histImg );
		
		namedWindow( "BackProjMorphed", 1 );
		imshow( "BackProjMorphed", backProj );
		
		//NOTE:
		
		waitKey();
		
	} else {
		
		std::cout << "no face found" << std::endl;
		
	}
}


// based on code from camshift.cpp

Point initialLocationGuess( const Mat& probImage ) {
	
	
	std::cout << "got to 1" << std::endl;
	
	//FIX investigate correct Matrix conversion
	
	CvMat _probImage = probImage;

	CvMoments moments;
	CvMat  stub, *mat = (CvMat*) &_probImage;
		//stub is used as a tmp buffer just for the copying of imgProb header info to mat

	moments.m00 = moments.m10 = moments.m01 = 0;
	
	std::cout << "got to 1A" << std::endl;

	mat = cvGetMat( mat, &stub );
	
	std::cout << "got to 2" << std::endl;

//FIX: throw exception !?
//	if( CV_MAT_CN( mat->type ) > 1 )
//		CV_ERROR( CV_BadNumChannels, cvUnsupportedFormat );

	int cx, cy;
	double inv_m00;

	cvMoments( mat, &moments );

	inv_m00 = moments.inv_sqrt_m00*moments.inv_sqrt_m00;
	cx = cvRound( moments.m10 * inv_m00);
	cy = cvRound( moments.m01 * inv_m00);

	return Point(cx,cy);
}

//namespace hi {
//		
//	Point initialLocationGuess( const Mat& probImage ) {
//		
//		CvMat _probImage = probImage;
//		
//		//FIX : TEST?!
//		return (Point) hiInitialLocationGuess( &_probImage );
//	}
//}

/*M///////////////////////////////////////////////////////////////////////////////////////
 //
 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 //
 //  By downloading, copying, installing or using the software you agree to this license.
 //  If you do not agree to this license, do not download, install,
 //  copy or use the software.
 //
 //
 //                        Intel License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2000, Intel Corporation, all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
 // are permitted provided that the following conditions are met:
 //
 //   * Redistribution's of source code must retain the above copyright notice,
 //     this list of conditions and the following disclaimer.
 //
 //   * Redistribution's in binary form must reproduce the above copyright notice,
 //     this list of conditions and the following disclaimer in the documentation
 //     and/or other materials provided with the distribution.
 //
 //   * The name of Intel Corporation may not be used to endorse or promote products
 //     derived from this software without specific prior written permission.
 //
 // This software is provided by the copyright holders and contributors "as is" and
 // any express or implied warranties, including, but not limited to, the implied
 // warranties of merchantability and fitness for a particular purpose are disclaimed.
 // In no event shall the Intel Corporation or contributors be liable for any direct,
 // indirect, incidental, special, exemplary, or consequential damages
 // (including, but not limited to, procurement of substitute goods or services;
 // loss of use, data, or profits; or business interruption) however caused
 // and on any theory of liability, whether in contract, strict liability,
 // or tort (including negligence or otherwise) arising in any way out of
 // the use of this software, even if advised of the possibility of such damage.
 //
 //M*/




